import queue
import time

import numpy as np
from utils.dijkstra import shortest_path


class PathPlanning:
    def __init__(self, start_node, goal_node, key_nodes, key_id, cost_v, edges, cost):
        self.start_node = start_node  # 起始点id（这里为索引）
        self.goal_node = goal_node
        self.key_nodes = key_nodes  # 关键节点id（这里为索引）
        # self.key_id = key_id
        self.cost_v = cost_v.astype(np.float)  # 节点权重
        self.edges = edges  # 存储边的有无
        self.cost = cost.astype(np.float)
        self.dim_cost = cost.shape[0]
        self.dim_n = cost_v.shape[1]  # 节点数量
        self.dim_k = len(key_nodes)  # 关键点数量（不包括起始点）

    def get_path(self):
        # 清完善代码获取正确的paths-------------------------------------------------------------------------

        paths = [
            {
                'nodes': ['node-2', 'node-0', 'node-1731', 'node-0', 'node-7'],
                'edges': ['edge-4', 'edge-1143', 'edge-1143', 'edge-7'],
                'cost': [414.0, 414.0]
            }
        ]
        return paths

    # 路径规划
    def path_planning(self):
        start_time = time.time()  # 记录开始时间
        # 路径重构
        paths = self.get_path()

        end_time = time.time()  # 记录结束时间
        elapsed_time = end_time - start_time  # 计算耗时
        print("*****************A*算法*********************")
        print(f"总耗时: {elapsed_time:.2f} s")
        print(f"最短路径长度: {paths[0]['cost']}")
        print(" ".join(map(str, paths[0]['path'])))
        print("********************************************")

        print(paths)
        return paths
